//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
//
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  Updated Nov-2007 by Trevor Taylor, Software Technology, Australia
//
//  Change Log:
//  Fixed a bug - the right bumper was not being added to the sensor array
//
//  $File: BSBumper.cs $ $Revision: 19 $
//-----------------------------------------------------------------------

using System;
using System.Net;
using System.Collections.Generic;
using System.ComponentModel;
using System.Security.Permissions;

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

using boe = Microsoft.Robotics.Services.BasicStamp2.Proxy;
using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;


namespace Microsoft.Robotics.Services.BasicStamp2.Bumper
{
    
    [Contract(Contract.Identifier)]
    [AlternateContract(bumper.Contract.Identifier)]
    [DisplayName("Boe-Bot Generic Contact Sensor")]
    [Description("Provides access to the Parallax BASIC Stamp 2 Boe-Bot infrared sensor used as a bumper.\n(Uses Generic Contact Sensors contract.)")]
    public class BumperService : DsspServiceBase
    {
        [EmbeddedResource("Microsoft.Robotics.Services.BasicStamp2.BoeBotBumper.xslt")]
        string _transform = null;
        
        [InitialStatePartner(Optional=true)]
        private bumper.ContactSensorArrayState _state = new bumper.ContactSensorArrayState();        

        [ServicePort("/BSBumper", AllowMultipleInstances=true)]
        private bumper.ContactSensorArrayOperations _mainPort = new bumper.ContactSensorArrayOperations();

        [Partner("BASICStamp2", Contract = boe.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
        private boe.BasicStamp2Operations _boebotPort = new boe.BasicStamp2Operations();

        [Partner("SubMgr", Contract=submgr.Contract.Identifier, CreationPolicy=PartnerCreationPolicy.CreateAlways, Optional=false)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();
        
        public BumperService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        /// <summary>
        /// Start Handler
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            // Subscribe to core BasicStamp2 board for bumper notifications
            SubscribeToBasicStamp2();

            base.Start();

        }

        /// <summary>
        /// Receive infrared notifications from the Boe-Bot service
        /// </summary>
        /// <param name="notification"></param>
        private void SensorsChangedHandler(boe.SensorsChanged notification)
        {
            foreach (bumper.ContactSensor bumper in _state.Sensors)
            {
                bool changed = false;
                if (bumper.HardwareIdentifier == 1 && bumper.Pressed != notification.Body.IRLeft)
                {
                    bumper.Pressed = (notification.Body.IRLeft);
                    changed = true;
                }

                if (bumper.HardwareIdentifier == 2 && bumper.Pressed != notification.Body.IRRight)
                {
                    bumper.Pressed = (notification.Body.IRRight);
                    changed = true;
                }

                if (bumper.HardwareIdentifier == 3 && bumper.Pressed != notification.Body.WhiskerLeft)
                {
                    bumper.Pressed = (notification.Body.WhiskerLeft);
                    changed = true;
                }

                if (bumper.HardwareIdentifier == 4 && bumper.Pressed != notification.Body.WhiskerRight)
                {
                    bumper.Pressed = (notification.Body.WhiskerRight);
                    changed = true;
                }

                if (changed)
                    this.SendNotification<bumper.Update>(_subMgrPort, new bumper.Update(bumper));

            }

        }

        #region Initialization

        /// <summary>
        /// Initialize State
        /// </summary>
        private void InitializeState()
        {
            bool changed = false;

            //configure initial state
            if (_state == null)
            {
                _state = new bumper.ContactSensorArrayState();
                changed = true;
            }

            if (_state.Sensors == null)
            {
                _state.Sensors = new List<bumper.ContactSensor>();
                changed = true;
            }
            if (_state.Sensors.Count == 0)
            {
                bumper.ContactSensor leftBumper = new bumper.ContactSensor();
                leftBumper.HardwareIdentifier = 1;
                leftBumper.Name = "Left InfraRed Bumper";

                _state.Sensors.Add(leftBumper);

                bumper.ContactSensor rightBumper = new bumper.ContactSensor();
                rightBumper.HardwareIdentifier = 2;
                rightBumper.Name = "Right InfraRed Bumper";

                // TT Oct-2007 - This was missing!
                _state.Sensors.Add(rightBumper);

                bumper.ContactSensor leftWhisker = new bumper.ContactSensor();
                leftWhisker.HardwareIdentifier = 3;
                leftWhisker.Name = "Left Whisker";

                _state.Sensors.Add(leftWhisker);

                bumper.ContactSensor rightWhisker = new bumper.ContactSensor();
                rightWhisker.HardwareIdentifier = 4;
                rightWhisker.Name = "Right Whisker";

                _state.Sensors.Add(rightWhisker);
                changed = true;
            }

            if (changed)
                SaveState(_state);

        }


        /// <summary>
        /// Subscribe to the Base Stamp 2
        /// </summary>
        private void SubscribeToBasicStamp2()
        {
            // Create a notification port
            boe.BasicStamp2Operations notificationPort = new boe.BasicStamp2Operations();

            // Subscribe to the basic stamp and wait for a response
            Activate(
                Arbiter.Choice(_boebotPort.Subscribe(notificationPort),
                    delegate(SubscribeResponseType Rsp)
                    {
                        // Note: To manage concurrency correctly between our main handlers and
                        // notifications we combine the main interleave arbiter protecting our handlers
                        // with notification from the infra red device
                        base.MainPortInterleave.CombineWith(new Interleave(
                            new ExclusiveReceiverGroup(
                                Arbiter.Receive<boe.SensorsChanged>(true, notificationPort, SensorsChangedHandler)
                            ), new ConcurrentReceiverGroup()
                        ));
                    },
                    delegate(Fault fault)
                    {
                        LogError("Subscription failed", fault);
                    }
                )
            );

        }
        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual void GetHandler(bumper.Get get)
        {
            get.ResponsePort.Post(_state);
        }

        /// <summary>
        /// Http Get Handler
        /// </summary>
        /// <param name="httpGet"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void ReplaceHandler(bumper.Replace replace)
        {
            // Save the new state
            _state = replace.Body;

            // Update all of the subscribers too
            foreach (bumper.ContactSensor bumper in _state.Sensors)
                SendNotification<bumper.Update>(_subMgrPort, new bumper.Update(bumper));

            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void SubscribeHandler(bumper.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            foreach (bumper.ContactSensor bumper in _state.Sensors)
            {
                // TT Jul-2008 - Change for July CTP of 2.0
                SendNotificationToTarget<bumper.Update>(subscribe.Body.Subscriber, _subMgrPort,
                        new bumper.Update(bumper));
            }
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void ReliableSubscribeHandler(bumper.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            foreach (bumper.ContactSensor bumper in _state.Sensors)
            {
                // TT Jul-2008 - Change for July CTP of 2.0
                SendNotificationToTarget<bumper.Update>(subscribe.Body.Subscriber, _subMgrPort,
                        new bumper.Update(bumper));
            }
        }

        /// <summary>
        /// Upate Notification
        /// </summary>
        /// <param name="updateBumper"></param>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void UpdateHandler(bumper.Update updateBumper)
        {
            throw new InvalidOperationException("Basic Stamp Sensors are not updateable");
        }
        #endregion
    }

    /// <summary>
    /// BasicStamp2 Bumper Contract
    /// </summary>
    public static class Contract
    {
        // TT - Modified the Contract Identifier for new version
        public const string Identifier = "http://schemas.microsoft.com/robotics/2007/10/bsbumper.html";
    }
}
